import cv2 as cv
import time
import numpy as np


def rotate_frame(frame, angle):
    (h, w) = frame.shape[:2]
    center = (w // 2, h // 2)
    M = cv.getRotationMatrix2D(center, angle, 1)
    return cv.warpAffine(frame, M, (w, h))


class CameraFace:
    def __init__(self, width=240, height=180, variety=0.2, nod_interval=0.5, shake_interval=1.0):
        self.face_det = cv.CascadeClassifier('lbpcascade_frontalface_improved.xml')
        self.cap = cv.VideoCapture(0, cv.CAP_DSHOW)
        self.width = width
        self.height = height
        self.variety = variety
        self.nod_interval = nod_interval
        self.shake_interval = shake_interval
        self.x, self.y = 0.5, 0.5
        self.nod_states = []
        self.shake_states = []
        self.latest_face = None

    def update(self, show_image=False):
        ret, frame = self.cap.read()
        frame = cv.resize(frame, dsize=(self.width, self.height))
        frame = cv.flip(frame, 1)
        if show_image:
            cv.imshow('video', frame)
        frame = np.concatenate((rotate_frame(frame, -30), frame, rotate_frame(frame, 30)))
        faces = self.face_det.detectMultiScale(frame)
        face = self._update_face_pos(faces)
        if face is not None:
            self.latest_face = frame[face[1]:face[1]+face[3], face[0]:face[0]+face[2]]
        self._update_face_nod(faces)
        self._update_face_shake(faces)
        return frame

    def _update_face_pos(self, faces):
        face = None
        area_max = 0
        for f in faces:
            if f[1] < self.height or f[1] > self.height * 2:
                continue
            area = f[2] * f[3]
            if area > area_max:
                area_max = area
                face = f
        if face is not None:
            x, y = face[0]/(self.width-face[2]), (face[1]-self.height)/(self.height-face[3])
            self.x = self.x * (1-self.variety) + x * self.variety
            self.y = self.y * (1-self.variety) + y * self.variety
            return face

    def _update_face_nod(self, faces):
        now_time = time.time()
        nod_state = 1 if len(faces) == 0 else 0
        self.nod_states.append([now_time, nod_state])
        self.nod_states = [x for x in self.nod_states if now_time - x[0] < self.nod_interval]

    def _update_face_shake(self, faces):
        now_time = time.time()
        for f in faces:
            state = 0
            if f[1] < self.height:
                state = -1
            elif f[1] > self.height*2:
                state = 1
            self.shake_states.append([time.time(), state])
        self.shake_states = [x for x in self.shake_states if now_time - x[0] < self.shake_interval]
           
    def get_face_pos(self):
        return self.x, self.y

    def has_noded(self):
        noded = False
        states = [x[1] for x in self.nod_states]
        if len(states) > 0 and states[0] == 0 and sum(states) == len(states) - 1:
            noded = True
            self.nod_states = []
        return noded

    def has_shaked(self):
        shaked = False
        states = set([x[1] for x in self.shake_states])
        if len(states) == 3:
            shaked = True
            self.shake_states = []
        return shaked

    def close(self):
        self.cap.release()

    def save_face_to_file(self, filename, size=(64,64)):
        if self.latest_face is not None:
            cv.imwrite(filename, cv.resize(self.latest_face, size))


def test():
    cf = CameraFace()
    while True:
        frame = cf.update()
        cv.imshow('video', frame)
#        x, y = cf.get_face_pos()
#        print(x, y)
        if cf.has_noded():
            print(time.time(), 'noded')
        if cf.has_shaked():
            print(time.time(), 'shaked')
        if cv.waitKey(40) == 27:
            break
    cf.save_face_to_file('face.png')
